#pragma once
#include <rtthread.h>

const int fifo_size = 16;

template<typename T>
class robot_fifo
{
public:
    robot_fifo(){
        head = 0;
        rear = 0;
        mut = rt_mutex_create("fifo", RT_IPC_FLAG_FIFO);
    };


    ~robot_fifo(){
        head = 0;
        rear = 0;
        rt_mutex_delete(mut);
    };

    void push(T d){
        int test = rear;
        test++;
        test %= fifo_size;
        if(test == head)
        {
            rt_kprintf("fifo is full, abandon data");
            return;
        }
        rt_mutex_take(mut, RT_WAITING_FOREVER);
        data[rear] = d;
        rear++;
        rear %= fifo_size;
        rt_mutex_release(mut);
    }

    T pop(void){
        T ret;
        if(head == rear)
        {
            rt_kprintf("fifo is empty");
            return ret;
        }
        rt_mutex_take(mut, RT_WAITING_FOREVER);
        ret = data[head];
        head++;
        head %= fifo_size;
        rt_mutex_release(mut);
        return ret;
    }

    void clear(void){
        rt_mutex_take(mut, RT_WAITING_FOREVER);
        head = rear;
        rt_mutex_release(mut);
    }

private:
    int head;
    int rear;
    T data[fifo_size];
    rt_mutex_t mut;
};
